//#error BP算法反向调节错误

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#define SAMPLE 100
#define INPUT 2
#define HIDE 3
#define OUTPUT 1
#define LOG(x) printf("[tran_times: %ld]%f\n",tran_times, (x))
#define P 0
#define I 1
#define D 2

#define ETA_O 0.02
#define ETA_H 0.1
#define OUT_MAX 1
#define OUT_MIN 0

#define INPUT_MIN -180
#define INPUT_MAX 180
#define PID_MIN 0
#define PID_MAX 255
#define BIG_PID 1 //TARGET > ERROR
#define SMALL_PID -1 //TARGET < ERROR
// double sample[SAMPLE];
long tran_times = 0;
double net_h[HIDE];
double net_o;
double w_i_h[INPUT][HIDE];
double w_h_o[HIDE];
double i_r[SAMPLE];
double i_e[SAMPLE];
double out_h[HIDE];
double out_o = 0;
double y = 0;
double last_y;
double last_out_h[HIDE];
double last_out_o;
double last_net_h[HIDE];

double deNormailze(double v){
    return (((v - (0)) * (INPUT_MAX - INPUT_MIN)) / (1 - (0))) + INPUT_MIN;
}
double normailze(double v){
    return ((v - INPUT_MIN) * (1 - (0))) / (INPUT_MAX - INPUT_MIN) - 0;
}
double dePIDNormailze(double v){
    return (((v - (0)) * (PID_MAX - PID_MIN)) / (1 - (0))) + PID_MIN;
}

void init_nerual_network(){
#warning 参考论文PID 时滞系统Example
    last_y = 0;
    last_out_o = 0;
//    for (long i = 0; i < INPUT; ++i){
//        for(long J = 0; J < HIDE; ++J){
//            w_i_h[i][J] = rand() * 2.0 / RAND_MAX - 1;
//        }
//    }
    for (long J = 0; J < HIDE; ++J) {
        w_i_h[0][J] = 1;
    }
    
    for (long J = 0; J < HIDE; ++J) {
        w_i_h[1][J] = -1;
    }
    
    for(long J = 0; J < HIDE; ++J){
        w_h_o[J] = 0.01;
        last_out_h[J] = 0;
        last_net_h[J] = 0;
    }
}
void forware_compute(double p){
    //net_h
    for(long J = 0; J < HIDE; ++J){
        net_h[J] = BIG_PID * (p * w_i_h[0][J] + last_y * w_i_h[1][J]);
    }
    //out_h_P
    if (net_h[P] > 0 && net_h[P] < 1)
        out_h[P] = net_h[P];
    else if(net_h[P] >= 1)
        out_h[P] = 1;
    else
        out_h[P] = 0;
//    out_h[P] = net_h[P];
//    if (out_h[P] > 1) {
//        out_h[P] = 1;
//    }else if(out_h[P] < -1){
//        out_h[P] = -1;
//    }
    //out_h_I
//    if (net_h[I] > -1 && net_h[I] < 1)
//        out_h[I] = last_out_h[I] + net_h[I];
//    else if(net_h[I] >= 1)
//        out_h[I] = 1;
//    else
//        out_h[I] = -1;
    
    
//#error last_out_h 和out_h 在后面会相互赋值，那么我们需要确定积分项是全局误差积分还是单次计算积分！！！我们通过arduino 的PID库进行验证！！！！
//#error 注意采样时间！！！！！！！！！！！！
//#error 已验证，积分为全局积分！！！
    
    
    out_h[I] = last_out_h[I] + net_h[I];
    if (out_h[I] * w_h_o[I] > 1) {
        out_h[I] = 1;
    }else if (out_h[I] * w_h_o[I] < 0){
        out_h[I] = 0;
    }
//    if (out_h[I] > 1) {
//        out_h[I] = 1;
//    }else if (out_h[I] < 0){
//        out_h[I] = 0;
//    }
    
    //out_h_D
//    if (net_h[D] > -1 && net_h[D] < 1)
//        out_h[D] = net_h[D] - last_out_h[D];
//    else if(net_h[D] >= 1)
//        out_h[D] = 1;
//    else
//        out_h[D] = -1;
//    
    out_h[D] = - (last_net_h[D] - net_h[D]);
//    if (out_h[D] > 1) {
//        out_h[D] = 1;
//    }else if (out_h[D] < -1){
//        out_h[D] = -1;
//    }
    //net_o
#warning 修改了+=,sum
    double sum = 0;
    for(long J = 0; J < HIDE; ++J){
        sum += out_h[J] * w_h_o[J];
    }
    net_o = sum;
    //out_o
    if(net_o > 1)
        out_o = 1;
    else if (net_o < 0)
        out_o = 0;
    else
        out_o = net_o;
    //system_compute
    //y
    
}
void back_compute(double ir){
    //w_h_o
    double delta;
    for(long J = 0; J < HIDE; ++J){
        if (ir - y  == 0 || out_o - last_out_o == 0) {
            delta = 0;
//            return;
        }else{
            delta =  2 * (ir - y) * ((y - last_y) / (out_o - last_out_o));
            double dw_h_o = - delta * out_h[J];
            w_h_o[J] = w_h_o[J] - ETA_O * dw_h_o;
        }
    }
//    w_ir_h
    for(long J = 0; J < HIDE; ++J){
#warning 除数为零的判断
        double Delta;
        if (ir - y == 0 || net_h[J] - last_net_h[J] == 0) {
            Delta = 0;
        }else{
            Delta = (out_h[J] - last_out_h[J]) / (net_h[J] - last_net_h[J]);
        }
        double dw_i_h = - delta * w_h_o[J] * Delta * ir;
        w_i_h[0][J] = w_i_h[0][J] - ETA_H * dw_i_h;
    }
#warning 推导是否出错
//    w_ie_h
    for(long J = 0; J < HIDE; ++J){
#warning 除数为零的判断
        double Delta;
        if (y - last_y == 0 || net_h[J] - last_net_h[J] == 0) {
            Delta = 0;
        }else{
            Delta = (out_h[J] - last_out_h[J]) / (net_h[J] - last_net_h[J]);
        }
        double dw_i_h = - delta * w_h_o[J] * Delta * last_y;
        w_i_h[1][J] = w_i_h[1][J] - ETA_H * dw_i_h;
    }
}
void rest_bp(){
    //last
    for(long J = 0; J < HIDE; ++J){
        last_out_h[J] = out_h[J];
        last_net_h[J] = net_h[J];
    }
#warning out_o在这里之前赋值了吗？赋值了
    
    last_out_o = out_o;
    last_y = y;
}
int main(){
    init_nerual_network();
    while(1){
        forware_compute(normailze(50));
        //system use out_o
        LOG(dePIDNormailze(out_o));
        double ra = 40;
        printf("[target]%f", ra);
        y = normailze(ra);
        back_compute(normailze(50));
        rest_bp();
        // LOG(y);
        tran_times++;
//        if (tran_times == 3000){
//            break;
//        }
    }
//    double cc;
//    while (1) {
//        forware_compute(normailze(30));
//        //system use out_o
//        
//        double ra = ra + cc;
//        printf("[target]%f", ra);
//        LOG(out_o);
//        y = normailze(ra);
//        back_compute(30);
//        rest_bp();
//        
//        if (out_o - 0 < 0.0001) {
//            cc -= 0.0001;
//        }else{
//            cc += 0.0001;
//        }
//    }
//    while(1){
//        forware_compute(normailze(70));
//        //system use out_o
//        LOG(deNormailze(out_o));
//        ra -= 1;
//        printf("[target]%f", ra);
//        y = normailze(ra);
//        back_compute(normailze(70));
////        if (ra == -160) {
////            break;
////        }
//    }
    return 0;
}